#遍历文件夹读取txt文件
import os
import numpy as np
from tqdm import tqdm
'''
读取txt文件
将其转换为numpy数组
然后保存为npy文件
位姿文件形式为4*4的矩阵
'''
def get_length(folder):
    return len(os.listdir(folder))

def load_txt(folder,id):
    pose = np.loadtxt(folder + f"\{id}" + ".txt")
    pose = pose.reshape(3,4)
    pose = np.vstack((pose, [0, 0, 0, 1]))
    return pose

def load_txt_4x4(folder,id):
    pose = np.loadtxt(folder + f"\{id}" + ".txt")
    pose = pose.reshape(4,4)
    return pose


if __name__ == "__main__":
    poses = []
    folder = r"E:\pose\datasets\obj_ac\obj_000000\pose_halcon"#todo path to the folder
    length = get_length(folder)
    for i in tqdm(range(length)):
        pose = load_txt(folder,i)
        # pose = load_txt_4x4(folder,i)
        poses.append(pose)
        print(i)
    save_path = os.path.join(folder,'transforms.npy')
    np.save(save_path, poses)
    print(f"transforms has been saved in {save_path}")
    len(poses)
